/***************************************************************** 
 *
 * FILE        : Alt_functions.c
 * 
 * DESCRIPTION : This file includes functios for diferents tests  
 *               in the MPC5604B
 *	             
 *               
 * 
 * COPYRIGHT   :(c) 2009, Freescale & STMicroelectronics 
 * 
 * VERSION     : 01.00 
 * DATE        : 26 SEPTEMBER 2012 
 * AUTHOR      : UTCH RAPTOR TEAM
 * HISTORY     : 
 *               
 *
 ******************************************************************/ 
 
#include "MPC5604B_M27V.h"
#include "Pads_names.h"
#include "Var_declaration.h"

 //extern vuint32_t i,freq,pixel[128],pixel2[128],max,min,limit,limit_aux,aux,aux2,center,speed;
// extern vuint16_t nn;
 //extern vuint8_t pix,linear_flag;
 
 
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
 
 /*FUNCTION DELAY(ms)*/
void delay_ms(int m)
{

while(m)
{

for(i=0;i<=8000;i++)
{

}

m--;

}
i=0;
}


/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/

/*FUNCTION DELAY(us)*/
void delay_us(int m)
{

while(m)
{

for(i=0;i<=8;i++)
{

}
m--;
}
i=0;
}

/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
void Camera_frame(void)                                               /****/
{                                                                     /****/
pix=0;                                                                /****/
	CLK=1;                                                            /****/
delay_us(10);                                                         /****/
/////////////////////////////start, pixel 0 is saved                  /****/
                                                                      /****/
CLK=0;
delay_us(freq/2);
START=1;
delay_us(freq/2);
CLK=1;
delay_us(freq/2);
START=0;
delay_us(freq/4);
nn=ANP0;
speed=ANP2;
pixel[pix]=ANP0;
delay_us(freq/4);
CLK=0;
delay_us(freq);
/////////////////////////////read of 127 pixels
for(pix=1;pix<=127;pix++)
{
CLK=1;
delay_us(freq/2);
pixel[pix]=ANP0;
delay_us(freq/2);
CLK=0;
delay_us(freq);
}
}
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/

/////////////////////////////Get the black limit

void Black_limits(vuint8_t st,vuint8_t end)
{
max=0;
min=1023;
	
for(pix=st;pix<=end;pix++)
{
if(pixel[pix]>max)
{
	max=pixel[pix];
}
if(pixel[pix]<min)
{
	min=pixel[pix];
}

}
limit_aux=(max-min)/8;
limit_aux=limit_aux*7;
limit=min+limit_aux;
limit=800;

}

/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/

void Frame_cleaning(void)
{
	
for(pix=0;pix<=127;pix++)
{

if(pixel[pix]==1)
{
aux=pixel[pix-2]+pixel[pix-1]+pixel[pix]+pixel[pix+1]+pixel[pix+2];
if(aux<3)
{
pixel[pix]=0;
}

}
}

}


/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
void Frame_view(vuint8_t us)
{
	
LED3=1;
for(pix=20;pix<=109;pix++)
{
if(pixel[pix]==1)
{
	
LED4=1;
}

else
{
LED4=0;
}
delay_us(us);	
}
LED3=0;

}

/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/

void Center_detection(void)
{
int j=19;
aux=0;
aux2=0;

for(pix=0;pix<=89;pix++)
{
j++;
if(pixel[j]==1)
{

aux=aux+j+1;

aux2++;
}

}
center=aux/aux2;
center=center-19;


}
 
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/

 
void Linear_frame(void) 
{
	while(PUSH1==0)
{
for(pix=0;pix<=127;pix++)
{
if(pixel[pix]<1)
{
	pixel[pix]=1;
}
pixel2[pix]=max/pixel[pix];
}
linear_flag=1;
}

if(linear_flag==1)
{
for(pix=0;pix<=127;pix++)
{
pixel[pix]=pixel[pix]*pixel2[pix];
}

}
}

/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/

void Pixel_conversion(void)
{
	for(pix=0;pix<=127;pix++)
{
if(pixel[pix]>limit)
{
pixel[pix]=0;
}

else if(pixel[pix]<limit)
{
pixel[pix]=1;
}

}
}

/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/

void Speed_control(void/*vuint8_t low,vuint8_t high*/)
{
/*
	speed=ANP4;
while(speed>high || speed<low & aux==0 )
{
speed=ANP4;
if(speed>high && MOTORI>0)
{
	MOTORI=MOTORI-5;
	MOTORD=MOTORD-5;
}

if(speed<low && MOTORI<999)
{
	MOTORI=MOTORI+5;
	MOTORD=MOTORD+5;
}

else 
{
	aux=1;
}

}
aux=0;

*/
    nn=ANP4;
	MOTORI=ANP4;
	MOTORD=ANP4;
	limit_aux=ANP3;
}


/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/
/****--------------------->>>>>>>>*****<<<<<<<<------------------------****/


void Direction_control(void)
{
   error=set_point-center;
   
   if(error<0 && error!=65)     
 	  {
	   	 direction=0;
		 error=error/(-1);
	  }
  else if(error>0 && error!=65)     
      {
	 	 direction=1;
	  }
	  
/****************************************/	  
	if(error>0 && error<=10)
       {
          error_group=0;		
	   }
	if(error>10 && error<=20)
       {
          error_group=1;
          error=error-10;		
	   }	   
	if(error>20 && error<=30)
       {
          error_group=2;
          error=error-20;		
	   }
	if(error>30 && error<=45)
       {
          error_group=3;
          error=error-30;		
	   }
/****************************************/	   
	if(aux2<=10)
       {
		  if(direction==0)
		     {
		  	    correction=310;
		     }
		  if(direction==1)
		     {
		  	    correction=(-300);
		     }
		     
		  PWMSERVO=servo_center+correction;
	   }
	if(aux2>10)
 	   {
 	      switch(error_group)
 	      {
 	      	case 0:
 	      	   correction=error*4;
 	      	break;
 	      	case 1:
 	      	   correction=error*5+(40);
 	      	break;
 	      	case 2:
 	      	   correction=error*7+(90);
 	      	break;
 	      	case 3:
 	      	   correction=error*10+(160);
 	      	break;
 	      	default:
 	      	break;
 	      }
 	      
		  if(direction==0)
		     {
		  	    PWMSERVO=servo_center+correction;
		     }
		  if(direction==1)
		     {
		  	    PWMSERVO=servo_center-correction;
		     }
		
	   }

}

/****************************************/

void Light_module(void)
{

	LIGHT1=19999;
	LIGHT2=19999;
	LIGHT5=13000;
	while(FLAG1!=1)
   {
   }
FLAG1=1;
	
}
	
